%% Plot Halo Orbits in Lagrange point 1
clc;clear;
format longg
%% Inputs.
G           = 6.674e-11 * ((1/1000)^3); % Gravitational parameters

mass_central = 1.989e30;
%[] Mass of central planet

mass_moon = 5.972e24;         %Callisto
%[] mass of moons

DU = 151.73e6;           %Callisto
%[km] Semi-major axis of Moons, used as distance units for each CRTBP
%environment.

moonName = {'Luna'};
%[] Name of the Moons

thetao = 0;
%[] Initial position of the moon relative to the first point of aries

GM_central = mass_central*G;
%[] Gravitational parameters

GM_moon = mass_moon*G;
%[] Gravitational parameters

N = length(mass_moon);
%[] Number of Moons

u = zeros(N,1);
for ii = 1:N
    u(ii) = GM_moon(ii)/(GM_moon(ii)+GM_central);
end
%[] Gravataional ratio

TU = zeros(N,1);
VU = zeros(N,1);
for ii = 1:N
    TU(ii) = sqrt(DU(ii)^3/GM_central);
    VU(ii) = DU(ii)/TU(ii);
end
%[] Time constant for each crtbp system

theta_dot = zeros(1,N);
for ii = 1:length(theta_dot)
    theta_dot(ii) = sqrt(GM_central*DU(ii))/DU(ii)^2;
end
%[] Theta dot for each planet.

planetNumb = 1;
[L1,L2,L3,L4,L5] = librationPoints(u(planetNumb));
L_ = [L1,L2,L3,L4,L5];
for ii = 1:length(L_)
    L_pos = L_(:,ii);
    J_L(ii) = jacobiConst(L_pos,zeros(3,1),u(planetNumb));
end


%% Initial Guess
Xo = [0.991609864782743;0;1e-10;0;-0.00958617040514363;0];
%[] One of Lyapunov Orbit as initial guess of trajectory

xo = Xo(1);     yo = Xo(2);     zo = Xo(3);
vxo = Xo(4);    vyo = Xo(5);    vzo = Xo(6);
%[] Pre-allocate initial states.

tau = 3.1026/2;
%[] Orbital period initial guess.

options=odeset('RelTol',1e-14,'AbsTol',1e-22);
%[] Increase the Ode 45 tol

%% CHANGEABLE PARAMETERS.
NumbOrbit = 150;
%[] Number of orbit to propagate.

dzo = 0.0001;
%[] Family steps.
% Negative value for Northern Halo Orbit.
% Positive value for Southern Halo Orbit.
%% Main Loop

InitialConditions = zeros(6,NumbOrbit);
%[] Pre-allocate initial condition matrix

OrbitalPeriods = zeros(1,NumbOrbit);
%[] Pre allocate memory for orbital periods.

for ii = 1:NumbOrbit
    iter = 1;
    %[] Reset iteration numbers
    while 1
        to = [0,tau];
        [t,S] = ode113(@(t,S)CR3BP_n(t,S,u),to,Xo,options);
        S = S';
        t = t';
        %[] Integrate states.
        
        if t(end)<1e-1
            break
        end
        %[] If the integrated time is less than 1e-1, digerved. Break.
        
        STM = State2STM(t,S,u);
        %[] Calculate the state transition matrix along the trajectory
        
        Vector_Field = State2VectorField(S,u);
        %[] Determine the vector field at the end for flight time
        %correction
        
        DF = [STM(2,1),STM(2,5),Vector_Field(2);
              STM(4,1),STM(4,5),Vector_Field(4);
              STM(6,1),STM(6,5),Vector_Field(6)];
        %[] Define correction matrix
        
        D=inv(DF);
        %[] Invert matrix
        
        delXo = -D*[S(2,end);S(4,end);S(6,end)];
        %[] determine changes using...
            %   1. Terminal Y position
            %   2. Terminal X velocity
            %   3. Terminal Z velocity
       
        if norm(delXo) > 1e3
            break
        end
        %[] Break if correction is out of the orders of magnitude of
        %interest.
        
        Update = [Xo(1);Xo(5);tau] + delXo;
        %[] Update initial guess using differential correction
            %   1. New initial X position
            %   2. New initial Y Velcity
            %   3. New half of orbital period
        
        Xo = [Update(1);0;Xo(3);0;Update(2);0];
        %[] Reset states guess.
        
        tau = Update(3);
        %[] Reset half of orbital period guess.
        
        fprintf('%d th iteration\nTol: %2.5e\n\n',iter,norm(delXo));
        %[] Print number of iteration and the tolerance.
        
        if norm(delXo) < 1e-10
            fprintf('Converged at %d th iteration\n',iter);
            break
        end
        %[] If tolerance is met, break
        
        iter = iter + 1;
        %[] Increase iteration number
    end
    
    InitialConditions(1:6,ii) = Xo;
    %[] Save initial conditions
    
    OrbitalPeriods(ii) = tau;
    %[] Save half of orbital periods.
    
    Xo = Xo + [0;0;dzo;0;0;0];
    %[] Increase or decrease initial Z to find next halo orbit.
    
    if (Xo(1)<0.8)||(Xo(1)>1.1)
        ConvergedCount = ii-1;
        fprintf('Initial Condition Diverged at %d th iteration \n\n',iter);
        break
    %[] Break if initial condition is outside of the range of L1 and L2
    elseif OrbitalPeriods(ii) < 1
        fprintf('Orbital Period diverged at %d th iteration \n\n',iter);
        break
    %[] Break if Orbital period diverged.
    end
    
    ConvergedCount = ii;
    %[] Output converged cound for plotting.
end

%% Save data

IC = InitialConditions(:,1:ConvergedCount);
T = OrbitalPeriods(1:ConvergedCount);
save('SEL1_Southern_Halo.mat','IC','T');
% save('SEL1_Northern_Halo.mat','IC','T');

%% Plot result

fprintf('Plotting results...\n\n');
PlotCR3BPSunEarth_EarthCenterView_Multiple(InitialConditions(:,1:ConvergedCount),...
                                            OrbitalPeriods(1:ConvergedCount),...
                                            u,...
                                            ConvergedCount)

